/**
  ******************************************************************************
  * @file           : ANOv4.c
  * @brief          : 匿名地面站v4通信协议
  ******************************************************************************
  * @attention		
  *
  * @Logs:
  * Date           Author       Notes
  * 2022-04-09     李树益      第一个版本
  ******************************************************************************
  */

#include "ANOv4.h"
#include <stdlib.h>

//数据拆分宏定义，在发送大于1字节的数据类型时，比如int16、float等，需要把数据拆分成单独字节进行发送
#define BYTE0(dwTemp)       ( *( (char *)(&dwTemp)	  ) )
#define BYTE1(dwTemp)       ( *( (char *)(&dwTemp) + 1) )
#define BYTE2(dwTemp)       ( *( (char *)(&dwTemp) + 2) )
#define BYTE3(dwTemp)       ( *( (char *)(&dwTemp) + 3) )

/**
 * @brief 初始化ANO
 *
 * @param ano		匿名地面站
 * @param send		发送函数
 *
 * @return 无
 */
void ANO_Init(ANOv4 *ano, void (*send)(unsigned char *buf, unsigned short size))
{
    if(ano == NULL)
    {
        ano = malloc(sizeof(ANOv4));
    }
    ano->send = send;
#if ANO_RING_BUFF
    ring_buffer_creat(&ano->ringbuff, rx_buf, ANO_RX_BUF_LEN);
#endif
}

static void ANO_DT_Send_Check(ANOv4 *ano, unsigned char head, unsigned char check_sum)
{
	unsigned char data_to_send[7];	//发送数据缓存
	data_to_send[0]=0xAA;
	data_to_send[1]=0xAA;
	data_to_send[2]=0xEF;
	data_to_send[3]=2;
	data_to_send[4]=head;
	data_to_send[5]=check_sum;

	unsigned char sum = 0;
	for(unsigned char i=0;i<6;i++)
		sum += data_to_send[i];
	data_to_send[6]=sum;

	ano->send(data_to_send, 7);
}

/////////////////////////////////////////////////////////////////////////////////////
//Data_Receive_Prepare函数是协议预解析，根据协议的格式，将收到的数据进行一次格式性解析，格式正确的话再进行数据解析
//移植时，此函数应由用户根据自身使用的通信方式自行调用，比如串口每收到一字节数据，则调用此函数一次
//此函数解析出符合格式的数据帧后，会自行调用数据解析函数
void ANO_DT_Data_Receive_Prepare(ANOv4 *ano, unsigned char data)
{
	static unsigned char RxBuffer[50];
	static unsigned char _data_len = 0,_data_cnt = 0;
	static unsigned char state = 0;
	
	if(state==0&&data==0xAA)
	{
		state=1;
		RxBuffer[0]=data;
	}
	else if(state==1&&data==0xAF)
	{
		state=2;
		RxBuffer[1]=data;
	}
	else if(state==2&&data<0XF1)
	{
		state=3;
		RxBuffer[2]=data;
	}
	else if(state==3&&data<50)
	{
		state = 4;
		RxBuffer[3]=data;
		_data_len = data;
		_data_cnt = 0;
	}
	else if(state==4&&_data_len>0)
	{
		_data_len--;
		RxBuffer[4+_data_cnt++]=data;
		if(_data_len==0)
			state = 5;
	}
	else if(state==5)
	{
		state = 0;
		RxBuffer[4+_data_cnt]=data;
		ANO_DT_Data_Receive_Anl(RxBuffer,_data_cnt+5);
	}
	else
		state = 0;
}
/////////////////////////////////////////////////////////////////////////////////////
//Data_Receive_Anl函数是协议数据解析函数，函数参数是符合协议格式的一个数据帧，该函数会首先对协议数据进行校验
//校验通过后对数据进行解析，实现相应功能
//此函数可以不用用户自行调用，由函数Data_Receive_Prepare自动调用
void ANO_DT_Data_Receive_Anl(uint8_t *data_buf,uint8_t num)
{
	uint8_t sum = 0;
	for(uint8_t i=0;i<(num-1);i++)
		sum += *(data_buf+i);
	if(!(sum==*(data_buf+num-1)))		return;		//判断sum
	if(!(*(data_buf)==0xAA && *(data_buf+1)==0xAF))		return;		//判断帧头
	
	if(*(data_buf+2)==0X01)
	{
		if(*(data_buf+4)==0X01)
			ACC_calibrate = 1;
		if(*(data_buf+4)==0X02)
			GYRO_calibrate = 1;
		if(*(data_buf+4)==0X03)
		{
			ACC_calibrate = 1;		
			GYRO_calibrate = 1;			
		}
	}
	
	else if(*(data_buf+2)==0X02)
	{
		if(*(data_buf+4)==0X01)
		{
			f.send_pid1 = 1;
			f.send_pid2 = 1;
			f.send_pid3 = 1;
			f.send_pid4 = 1;
			f.send_pid5 = 1;
			f.send_pid6 = 1;
		}
		if(*(data_buf+4)==0X02)
		{
			
		}
		if(*(data_buf+4)==0XA0)		//读取版本信息
		{
			f.send_version = 1;
		}
		if(*(data_buf+4)==0XA1)		//恢复默认参数
		{
			OS_FLASH_Reset();
		}
		ANO_DT_Send_Check(*(data_buf+2),sum);
	}

	else if(*(data_buf+2)==0X10)								//PID1
	{
		OS_FLASH_Write_Start(10);
		
		flash_data.set.pid_kp[PID_ROLL_SPIEED]		= 0.001*( (int16_t)(*(data_buf+4)<<8)|*(data_buf+5) );
		flash_data.set.pid_ki[PID_ROLL_SPIEED]		= 0.001*( (int16_t)(*(data_buf+6)<<8)|*(data_buf+7) );
		flash_data.set.pid_kd[PID_ROLL_SPIEED]		= 0.001*( (int16_t)(*(data_buf+8)<<8)|*(data_buf+9) );
	
		flash_data.set.pid_kp[PID_PITCH_SPIEED]		= 0.001*( (int16_t)(*(data_buf+10)<<8)|*(data_buf+11) );
		flash_data.set.pid_ki[PID_PITCH_SPIEED]		= 0.001*( (int16_t)(*(data_buf+12)<<8)|*(data_buf+13) );
		flash_data.set.pid_kd[PID_PITCH_SPIEED]		= 0.001*( (int16_t)(*(data_buf+14)<<8)|*(data_buf+15) );
	
		flash_data.set.pid_kp[PID_YAW_SPIEED]		= 0.001*( (int16_t)(*(data_buf+16)<<8)|*(data_buf+17) );
		flash_data.set.pid_ki[PID_YAW_SPIEED]		= 0.001*( (int16_t)(*(data_buf+18)<<8)|*(data_buf+19) );
		flash_data.set.pid_kd[PID_YAW_SPIEED]		= 0.001*( (int16_t)(*(data_buf+20)<<8)|*(data_buf+21) );
		
		OS_FLASH_Write_End();
		
		ANO_DT_Send_Check(*(data_buf+2),sum);
	}
	else if(*(data_buf+2)==0X11)								//PID2
	{
		OS_FLASH_Write_Start(10);
		
		flash_data.set.pid_kp[PID_ROLL_ANGLE]		= 0.001*( (int16_t)(*(data_buf+4)<<8)|*(data_buf+5) );
		flash_data.set.pid_ki[PID_ROLL_ANGLE]		= 0.001*( (int16_t)(*(data_buf+6)<<8)|*(data_buf+7) );
		flash_data.set.pid_kd[PID_ROLL_ANGLE]		= 0.001*( (int16_t)(*(data_buf+8)<<8)|*(data_buf+9) );

		flash_data.set.pid_kp[PID_PITCH_ANGLE]		= 0.001*( (int16_t)(*(data_buf+10)<<8)|*(data_buf+11) );
		flash_data.set.pid_ki[PID_PITCH_ANGLE]		= 0.001*( (int16_t)(*(data_buf+12)<<8)|*(data_buf+13) );
		flash_data.set.pid_kd[PID_PITCH_ANGLE]		= 0.001*( (int16_t)(*(data_buf+14)<<8)|*(data_buf+15) );

		flash_data.set.pid_kp[PID_YAW_ANGLE]		= 0.001*( (int16_t)(*(data_buf+16)<<8)|*(data_buf+17) );
		flash_data.set.pid_ki[PID_YAW_ANGLE]		= 0.001*( (int16_t)(*(data_buf+18)<<8)|*(data_buf+19) );
		flash_data.set.pid_kd[PID_YAW_ANGLE]		= 0.001*( (int16_t)(*(data_buf+20)<<8)|*(data_buf+21) );
		
		OS_FLASH_Write_End();
		
		ANO_DT_Send_Check(*(data_buf+2),sum);

	}
	else if(*(data_buf+2)==0X12)								//PID3
	{	
		OS_FLASH_Write_Start(10);
		
		flash_data.set.pid_kp[PID_THR_SPIEED]		= 0.001*( (int16_t)(*(data_buf+4)<<8)|*(data_buf+5) );
		flash_data.set.pid_ki[PID_THR_SPIEED]		= 0.001*( (int16_t)(*(data_buf+6)<<8)|*(data_buf+7) );
		flash_data.set.pid_kd[PID_THR_SPIEED]		= 0.001*( (int16_t)(*(data_buf+8)<<8)|*(data_buf+9) );

		flash_data.set.pid_kp[PID_HIGHT_SPIEED]		= 0.001*( (int16_t)(*(data_buf+10)<<8)|*(data_buf+11) );
		flash_data.set.pid_ki[PID_HIGHT_SPIEED]		= 0.001*( (int16_t)(*(data_buf+12)<<8)|*(data_buf+13) );
		flash_data.set.pid_kd[PID_HIGHT_SPIEED]		= 0.001*( (int16_t)(*(data_buf+14)<<8)|*(data_buf+15) );

		flash_data.set.pid_kp[PID_GPS_SPIEED]		= 0.001*( (int16_t)(*(data_buf+16)<<8)|*(data_buf+17) );
		flash_data.set.pid_ki[PID_GPS_SPIEED]		= 0.001*( (int16_t)(*(data_buf+18)<<8)|*(data_buf+19) );
		flash_data.set.pid_kd[PID_GPS_SPIEED]		= 0.001*( (int16_t)(*(data_buf+20)<<8)|*(data_buf+21) );
		
		OS_FLASH_Write_End();
		
		ANO_DT_Send_Check(*(data_buf+2),sum);
	}
	else if(*(data_buf+2)==0X13)								//PID4
	{
		OS_FLASH_Write_Start(10);
		
		flash_data.set.pid_kp[PID_GPS_POS]		= 0.001*( (int16_t)(*(data_buf+4)<<8)|*(data_buf+5) );
		flash_data.set.pid_ki[PID_GPS_POS]		= 0.001*( (int16_t)(*(data_buf+6)<<8)|*(data_buf+7) );
		flash_data.set.pid_kd[PID_GPS_POS]		= 0.001*( (int16_t)(*(data_buf+8)<<8)|*(data_buf+9) );

		flash_data.set.pid_kp[PID_TEMPERATURE]		= 0.001*( (int16_t)(*(data_buf+10)<<8)|*(data_buf+11) );
		flash_data.set.pid_ki[PID_TEMPERATURE]		= 0.001*( (int16_t)(*(data_buf+12)<<8)|*(data_buf+13) );
		flash_data.set.pid_kd[PID_TEMPERATURE]		= 0.001*( (int16_t)(*(data_buf+14)<<8)|*(data_buf+15) );

		flash_data.set.pid_kp[11]		= 0.001*( (int16_t)(*(data_buf+16)<<8)|*(data_buf+17) );
		flash_data.set.pid_ki[11]		= 0.001*( (int16_t)(*(data_buf+18)<<8)|*(data_buf+19) );
		flash_data.set.pid_kd[11]		= 0.001*( (int16_t)(*(data_buf+20)<<8)|*(data_buf+21) );
		
		OS_FLASH_Write_End();
		
		ANO_DT_Send_Check(*(data_buf+2),sum);
	}
	else if(*(data_buf+2)==0X14)								//PID5
	{
		OS_FLASH_Write_Start(10);
		
		flash_data.set.pid_kp[12]		= 0.001*( (int16_t)(*(data_buf+4)<<8)|*(data_buf+5) );
		flash_data.set.pid_ki[12]		= 0.001*( (int16_t)(*(data_buf+6)<<8)|*(data_buf+7) );
		flash_data.set.pid_kd[12]		= 0.001*( (int16_t)(*(data_buf+8)<<8)|*(data_buf+9) );

		flash_data.set.pid_kp[13]		= 0.001*( (int16_t)(*(data_buf+10)<<8)|*(data_buf+11) );
		flash_data.set.pid_ki[13]		= 0.001*( (int16_t)(*(data_buf+12)<<8)|*(data_buf+13) );
		flash_data.set.pid_kd[13]		= 0.001*( (int16_t)(*(data_buf+14)<<8)|*(data_buf+15) );

		flash_data.set.pid_kp[14]		= 0.001*( (int16_t)(*(data_buf+16)<<8)|*(data_buf+17) );
		flash_data.set.pid_ki[14]		= 0.001*( (int16_t)(*(data_buf+18)<<8)|*(data_buf+19) );
		flash_data.set.pid_kd[14]		= 0.001*( (int16_t)(*(data_buf+20)<<8)|*(data_buf+21) );
		
		OS_FLASH_Write_End();
		
		ANO_DT_Send_Check(*(data_buf+2),sum);
	}
	else if(*(data_buf+2)==0X15)								//PID6
	{
		OS_FLASH_Write_Start(10);
		
		flash_data.set.pid_kp[15]		= 0.001*( (int16_t)(*(data_buf+4)<<8)|*(data_buf+5) );
		flash_data.set.pid_ki[15]		= 0.001*( (int16_t)(*(data_buf+6)<<8)|*(data_buf+7) );
		flash_data.set.pid_kd[15]		= 0.001*( (int16_t)(*(data_buf+8)<<8)|*(data_buf+9) );

		flash_data.set.pid_kp[16]		= 0.001*( (int16_t)(*(data_buf+10)<<8)|*(data_buf+11) );
		flash_data.set.pid_ki[16]		= 0.001*( (int16_t)(*(data_buf+12)<<8)|*(data_buf+13) );
		flash_data.set.pid_kd[16]		= 0.001*( (int16_t)(*(data_buf+14)<<8)|*(data_buf+15) );

		flash_data.set.pid_kp[17]		= 0.001*( (int16_t)(*(data_buf+16)<<8)|*(data_buf+17) );
		flash_data.set.pid_ki[17]		= 0.001*( (int16_t)(*(data_buf+18)<<8)|*(data_buf+19) );
		flash_data.set.pid_kd[17]		= 0.001*( (int16_t)(*(data_buf+20)<<8)|*(data_buf+21) );
		
		OS_FLASH_Write_End();
		
		ANO_DT_Send_Check(*(data_buf+2),sum);
	}
}

void ANO_DT_Send_Version(uint8_t hardware_type, uint16_t hardware_ver,uint16_t software_ver,uint16_t protocol_ver,uint16_t bootloader_ver)
{
	uint8_t data_to_send[14];	//发送数据缓存
	uint8_t _cnt=0;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x00;
	data_to_send[_cnt++]=0;
	
	data_to_send[_cnt++]=hardware_type;
	data_to_send[_cnt++]=BYTE1(hardware_ver);
	data_to_send[_cnt++]=BYTE0(hardware_ver);
	data_to_send[_cnt++]=BYTE1(software_ver);
	data_to_send[_cnt++]=BYTE0(software_ver);
	data_to_send[_cnt++]=BYTE1(protocol_ver);
	data_to_send[_cnt++]=BYTE0(protocol_ver);
	data_to_send[_cnt++]=BYTE1(bootloader_ver);
	data_to_send[_cnt++]=BYTE0(bootloader_ver);
	
	data_to_send[3] = _cnt-4;
	
	uint8_t sum = 0;
	for(uint8_t i=0;i<_cnt;i++)
		sum += data_to_send[i];
	data_to_send[_cnt++]=sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Status(float angle_rol, float angle_pit, float angle_yaw, int32_t alt, uint8_t fly_model, uint8_t armed)
{
	uint8_t data_to_send[17];	//发送数据缓存
	uint8_t _cnt=0;
	int16_t _temp;
	__IO int32_t _temp2 = alt;
	
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x01;
	data_to_send[_cnt++]=0;
	
	_temp = (int)(angle_rol*100);
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = (int)(angle_pit*100);
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = (int)(angle_yaw*100);
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	
	data_to_send[_cnt++]=BYTE3(_temp2);
	data_to_send[_cnt++]=BYTE2(_temp2);
	data_to_send[_cnt++]=BYTE1(_temp2);
	data_to_send[_cnt++]=BYTE0(_temp2);
	
	data_to_send[_cnt++] = fly_model;
	
	data_to_send[_cnt++] = armed;
	
	data_to_send[3] = _cnt-4;
	
	uint8_t sum = 0;
	for(uint8_t i=0;i<_cnt;i++)
		sum += data_to_send[i];
	data_to_send[_cnt++]=sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Senser(int16_t a_x,int16_t a_y,int16_t a_z,int16_t g_x,int16_t g_y,int16_t g_z,int16_t m_x,int16_t m_y,int16_t m_z,int32_t bar)
{
	uint8_t data_to_send[23];	//发送数据缓存
	uint8_t _cnt=0;
	int16_t _temp;
	
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x02;
	data_to_send[_cnt++]=0;
	
	_temp = a_x;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = a_y;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = a_z;	
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	
	_temp = g_x;	
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = g_y;	
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = g_z;	
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	
	_temp = m_x;	
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = m_y;	
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = m_z;	
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	
	data_to_send[3] = _cnt-4;
	
	uint8_t sum = 0;
	for(uint8_t i=0;i<_cnt;i++)
		sum += data_to_send[i];
	data_to_send[_cnt++] = sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_RCData(uint16_t thr,uint16_t yaw,uint16_t rol,uint16_t pit,uint16_t aux1,uint16_t aux2,uint16_t aux3,uint16_t aux4,uint16_t aux5,uint16_t aux6)
{
	uint8_t data_to_send[25];	//发送数据缓存
	uint8_t _cnt=0;
	
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x03;
	data_to_send[_cnt++]=0;
	data_to_send[_cnt++]=BYTE1(thr);
	data_to_send[_cnt++]=BYTE0(thr);
	data_to_send[_cnt++]=BYTE1(yaw);
	data_to_send[_cnt++]=BYTE0(yaw);
	data_to_send[_cnt++]=BYTE1(rol);
	data_to_send[_cnt++]=BYTE0(rol);
	data_to_send[_cnt++]=BYTE1(pit);
	data_to_send[_cnt++]=BYTE0(pit);
	data_to_send[_cnt++]=BYTE1(aux1);
	data_to_send[_cnt++]=BYTE0(aux1);
	data_to_send[_cnt++]=BYTE1(aux2);
	data_to_send[_cnt++]=BYTE0(aux2);
	data_to_send[_cnt++]=BYTE1(aux3);
	data_to_send[_cnt++]=BYTE0(aux3);
	data_to_send[_cnt++]=BYTE1(aux4);
	data_to_send[_cnt++]=BYTE0(aux4);
	data_to_send[_cnt++]=BYTE1(aux5);
	data_to_send[_cnt++]=BYTE0(aux5);
	data_to_send[_cnt++]=BYTE1(aux6);
	data_to_send[_cnt++]=BYTE0(aux6);

	data_to_send[3] = _cnt-4;
	
	uint8_t sum = 0;
	for(uint8_t i=0;i<_cnt;i++)
		sum += data_to_send[i];
	
	data_to_send[_cnt++]=sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Power(uint16_t votage, uint16_t current)
{
	uint8_t data_to_send[9];	//发送数据缓存
	uint8_t _cnt=0;
	uint16_t temp;
	
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x05;
	data_to_send[_cnt++]=0;
	
	temp = votage;
	data_to_send[_cnt++]=BYTE1(temp);
	data_to_send[_cnt++]=BYTE0(temp);
	temp = current;
	data_to_send[_cnt++]=BYTE1(temp);
	data_to_send[_cnt++]=BYTE0(temp);
	
	data_to_send[3] = _cnt-4;
	
	uint8_t sum = 0;
	for(uint8_t i=0;i<_cnt;i++)
		sum += data_to_send[i];
	
	data_to_send[_cnt++]=sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_MotoPWM(uint16_t m_1,uint16_t m_2,uint16_t m_3,uint16_t m_4,uint16_t m_5,uint16_t m_6,uint16_t m_7,uint16_t m_8)
{
	uint8_t data_to_send[21];	//发送数据缓存
	uint8_t _cnt=0;
	
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x06;
	data_to_send[_cnt++]=0;
	
	data_to_send[_cnt++]=BYTE1(m_1);
	data_to_send[_cnt++]=BYTE0(m_1);
	data_to_send[_cnt++]=BYTE1(m_2);
	data_to_send[_cnt++]=BYTE0(m_2);
	data_to_send[_cnt++]=BYTE1(m_3);
	data_to_send[_cnt++]=BYTE0(m_3);
	data_to_send[_cnt++]=BYTE1(m_4);
	data_to_send[_cnt++]=BYTE0(m_4);
	data_to_send[_cnt++]=BYTE1(m_5);
	data_to_send[_cnt++]=BYTE0(m_5);
	data_to_send[_cnt++]=BYTE1(m_6);
	data_to_send[_cnt++]=BYTE0(m_6);
	data_to_send[_cnt++]=BYTE1(m_7);
	data_to_send[_cnt++]=BYTE0(m_7);
	data_to_send[_cnt++]=BYTE1(m_8);
	data_to_send[_cnt++]=BYTE0(m_8);
	
	data_to_send[3] = _cnt-4;
	
	uint8_t sum = 0;
	for(uint8_t i=0;i<_cnt;i++)
		sum += data_to_send[i];
	
	data_to_send[_cnt++]=sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_PID(uint8_t group,float p1_p,float p1_i,float p1_d,float p2_p,float p2_i,float p2_d,float p3_p,float p3_i,float p3_d)
{
	uint8_t data_to_send[23];	//发送数据缓存
	uint8_t _cnt=0;
	__IO int16_t _temp;
	
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x10+group-1;
	data_to_send[_cnt++]=0;
	
	
	_temp = p1_p * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p1_i  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p1_d  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p2_p  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p2_i  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p2_d * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p3_p  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p3_i  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p3_d * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	
	data_to_send[3] = _cnt-4;
	
	uint8_t sum = 0;
	for(uint8_t i=0;i<_cnt;i++)
		sum += data_to_send[i];
	
	data_to_send[_cnt++]=sum;

	ANO_DT_Send_Data(data_to_send, _cnt);
}

